#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
ROS Noetic 测试脚本：
持续发布 map->body TF
轨迹：从 (0,0) 出发，沿着 r = θ 的螺旋线慢慢远离原点
"""
import rospy
import math
import tf2_ros
from geometry_msgs.msg import TransformStamped


def spiral_broadcaster():
    rospy.init_node("pcd_loader_test_tf_broadcaster")
    br = tf2_ros.TransformBroadcaster()
    t = 0.0
    dt = rospy.get_param("~dt", 0.1)  # 时间步长 (rad)
    rate_hz = rospy.get_param("~rate", 20.0)  # 广播频率 (Hz)
    rate = rospy.Rate(rate_hz)

    while not rospy.is_shutdown():
        # 计算螺旋坐标： r = t, theta = t
        r = t
        theta = t**0.7
        x = r * math.cos(theta)
        y = r * math.sin(theta)
        z = 0.0

        # 构造 TransformStamped
        tf_msg = TransformStamped()
        tf_msg.header.stamp = rospy.Time.now()
        tf_msg.header.frame_id = "map"
        tf_msg.child_frame_id = "body"
        tf_msg.transform.translation.x = x
        tf_msg.transform.translation.y = y
        tf_msg.transform.translation.z = z
        # 无旋转，仅平移
        tf_msg.transform.rotation.x = 0.0
        tf_msg.transform.rotation.y = 0.0
        tf_msg.transform.rotation.z = 0.0
        tf_msg.transform.rotation.w = 1.0

        # 发布 TF
        br.sendTransform(tf_msg)

        # 增加参数 t
        t += dt
        rate.sleep()


if __name__ == "__main__":
    try:
        spiral_broadcaster()
    except rospy.ROSInterruptException:
        pass
